﻿/*****************************************************************************

Copyright (c) 2004 SensAble Technologies, Inc. All rights reserved.

OpenHaptics(TM) toolkit. The material embodied in this software and use of
this software is subject to the terms and conditions of the clickthrough
Development License Agreement.

For questions, comments or bug reports, go to forums at:
	http://dsc.sensable.com

Module Name:

  QueryDevice.c

Description:

  This example demonstrates how to retrieve information from the haptic device.
  错误地以为ur机器人的orientation是欧拉角，其实是旋转矢量，所以这个程序是有问题的

*******************************************************************************/
#ifdef  _WIN64
#pragma warning (disable:4996)
#endif

#if defined(WIN32)
# define NOMINMAX
# include <windows.h>
# include <conio.h>
#else
# include "conio.h"
# include <string.h>
#endif

#include <stdio.h>
#include <assert.h>

#include <HD/hd.h>

#include <HDU/hduVector.h>
#include <HDU/hduError.h>

#include <cmath>
#include <array>

#include <iostream>
#include <cstdlib>
#include <string>
#include <thread>
#include <atomic>
#include <chrono>
#include <cstring>
#include "mqtt/async_client.h"

#include <sstream>

#include "Eigen/Dense"
#include <iomanip>

using namespace Eigen;


const double _FLOAT_EPS = std::numeric_limits<double>::epsilon();
const double _EPS4 = _FLOAT_EPS * 4.0;

using namespace std;

const string DFLT_SERVER_ADDRESS{ "tcp://localhost:1883" };
//const string DFLT_SERVER_ADDRESS{ "broker.emqx.io:1883" };
const string CLIENT_ID{ "paho_cpp_async_publish" };
const string PERSIST_DIR{ "./persist" };

const string TOPIC{ "ee_pose" };

const char* LWT_PAYLOAD = "Last will and testament.";

const int  QOS = 0;

const auto TIMEOUT = std::chrono::seconds(10);

// 将设备返回的数据映射到Eigen的Matrix3d类型
Matrix3d rawdataToMatrix(HDfloat transform[16]) {
	Matrix3d rotationMatrix;
	rotationMatrix << transform[0], transform[4], transform[8],
		transform[1], transform[5], transform[9],
		transform[2], transform[6], transform[10];

	return rotationMatrix;
}

Vector3d rotationMatrixToVector(const Matrix3d& R) {
	AngleAxisd angleAxis(R);  // 使用AngleAxis类从旋转矩阵构造

	// AngleAxis类包含了旋转轴和旋转角信息
	Vector3d axis = angleAxis.axis();  // 旋转轴
	double angle = angleAxis.angle();  // 旋转角

	// 将旋转轴和角度组合成旋转矢量
	Vector3d rotationVector = axis * angle;

	return rotationVector;
}





//3*3 rotation matrix to euler angles
//3*3旋转矩阵转欧拉角
std::array<double, 3> mat2euler(std::array<std::array<double, 3>, 3> mat) {
	double cy = std::sqrt(mat[2][2] * mat[2][2] + mat[1][2] * mat[1][2]);
	bool condition = cy > _EPS4;
	std::array<double, 3> euler;
	euler[2] = condition ? -std::atan2(mat[0][1], mat[0][0]) : -std::atan2(-mat[1][0], mat[1][1]);
	euler[1] = condition ? -std::atan2(-mat[0][2], cy) : -std::atan2(-mat[0][2], cy);
	euler[0] = condition ? -std::atan2(mat[1][2], mat[2][2]) : 0.0;
	return euler;
}

//data from the device is a vector of size 16 , and is stored in column-major order.
//设备返回的数据是一个16维向量，按列存储
std::array<std::array<double, 3>, 3> rawdata2mat(HDfloat transform[16]) {
	std::array<std::array<double, 3>, 3> mat;
	mat[0][0] = transform[0];
	mat[0][1] = transform[4];
	mat[0][2] = transform[8];
	mat[1][0] = transform[1];
	mat[1][1] = transform[5];
	mat[1][2] = transform[9];
	mat[2][0] = transform[2];
	mat[2][1] = transform[6];
	mat[2][2] = transform[10];
	return mat;
}

//get the end effector's euler orientation from the device
//从设备获取末端执行器的姿态
std::array<double, 3> rawdata2euler(HDfloat transform[16]) {
	std::array<std::array<double, 3>, 3> mat = rawdata2mat(transform);
	return mat2euler(mat);
}

//get the end effector's pose from the device
//从设备获取末端执行器的位姿
std::array<double, 6> rawdata2pose(HDfloat transform[16]) {
	std::array<double, 6> pose;
	pose[0] = transform[12];
	pose[1] = transform[13];
	pose[2] = transform[14];
	std::array<double, 3> euler = rawdata2euler(transform);
	pose[3] = euler[0];
	pose[4] = euler[1];
	pose[5] = euler[2];
	return pose;
}

std::string vectorToString(const std::array<double, 6>& myArray) {
	// 使用 std::ostringstream 构建字符串
	std::ostringstream oss;
	for (const auto& value : myArray) {
		oss << value << " ";
	}

	// 将 std::ostringstream 的内容转换为 std::string
	return oss.str();
}

/* Holds data retrieved from HDAPI. */
typedef struct
{
	HDboolean m_buttonState;       /* Has the device button has been pressed. */
	hduVector3Dd m_devicePosition; /* Current device coordinates. */
	HDfloat m_deviceTransform[16];
	HDErrorInfo m_error;
} DeviceData;

typedef struct
{
	hduVector3Dd m_force;
	hduVector3Dd m_torque;
} ForceTorque;

static DeviceData gServoDeviceData;
static ForceTorque gForceTorqueData = {hduVector3Dd(0.0,0.0,0.0),hduVector3Dd(0.0,0.0,0.0)};

/*******************************************************************************
 Checks the state of the gimbal button and gets the position of the device.
*******************************************************************************/
HDCallbackCode HDCALLBACK updateDeviceCallback(void* pUserData)
{
	int nButtons = 0;

	hdBeginFrame(hdGetCurrentDevice());

	/* Retrieve the current button(s). */
	hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);

	/* In order to get the specific button 1 state, we use a bitmask to
	   test for the HD_DEVICE_BUTTON_1 bit. */
	gServoDeviceData.m_buttonState =
		(nButtons & HD_DEVICE_BUTTON_1) ? HD_TRUE : HD_FALSE;

	/* Get the current location of the device (HD_GET_CURRENT_POSITION)
	   We declare a vector of three doubles since hdGetDoublev returns
	   the information in a vector of size 3. */
	hdGetDoublev(HD_CURRENT_POSITION, gServoDeviceData.m_devicePosition);

	hdGetFloatv(HD_CURRENT_TRANSFORM, gServoDeviceData.m_deviceTransform);

	/* 设置机器人力反馈 */
	/*hdSetDoublev(HD_CURRENT_FORCE, gForceTorqueData.m_force);*/
	//hdSetDoublev(HD_CURRENT_TORQUE, gForceTorqueData.m_torque);

	/* Also check the error state of HDAPI. */
	gServoDeviceData.m_error = hdGetError();

	/* Copy the position into our device_data tructure. */
	hdEndFrame(hdGetCurrentDevice());

	return HD_CALLBACK_CONTINUE;
}


/*******************************************************************************
 Checks the state of the gimbal button and gets the position of the device.
*******************************************************************************/
HDCallbackCode HDCALLBACK copyDeviceDataCallback(void* pUserData)
{
	DeviceData* pDeviceData = (DeviceData*)pUserData;

	memcpy(pDeviceData, &gServoDeviceData, sizeof(DeviceData));

	return HD_CALLBACK_DONE;
}


/*******************************************************************************
 Prints out a help string about using this example.
*******************************************************************************/
void printHelp(void)
{
	static const char help[] = { "\
Press and release the stylus button to print out the current device location.\n\
Press and hold the stylus button to exit the application\n" };

	fprintf(stdout, "%s\n", help);
}


/*******************************************************************************
 This routine allows the device to provide information about the current
 location of the stylus, and contains a mechanism for terminating the
 application.
 Pressing the button causes the application to display the current location
 of the device.
 Holding the button down for N iterations causes the application to exit.
*******************************************************************************/
void mainLoop()
{
	static const int kTerminateCount = 1000;
	int buttonHoldCount = 0;

	/* Instantiate the structure used to capture data from the device. */
	DeviceData currentData;
	DeviceData prevData;

	/* Perform a synchronous call to copy the most current device state. */
	hdScheduleSynchronous(copyDeviceDataCallback,
		&currentData, HD_MIN_SCHEDULER_PRIORITY);

	memcpy(&prevData, &currentData, sizeof(DeviceData));

	printHelp();

	/* Run the main loop until the gimbal button is held. */
	while (1)
	{
		/* Perform a synchronous call to copy the most current device state.
		   This synchronous scheduler call ensures that the device state
		   is obtained in a thread-safe manner. */
		hdScheduleSynchronous(copyDeviceDataCallback,
			&currentData,
			HD_MIN_SCHEDULER_PRIORITY);

		/* If the user depresses the gimbal button, display the current
		   location information. */
		if (currentData.m_buttonState && !prevData.m_buttonState)
		{
			fprintf(stdout, "Current position: (%g, %g, %g)\n",
				currentData.m_devicePosition[0],
				currentData.m_devicePosition[1],
				currentData.m_devicePosition[2]
			);

			std::array<double, 3> euler = rawdata2euler(currentData.m_deviceTransform);
			fprintf(stdout, "Current euler: (%g, %g, %g)\n",
				euler[0],
				euler[1],
				euler[2]
			);

		}
		else if (currentData.m_buttonState && prevData.m_buttonState)
		{
			/* Keep track of how long the user has been pressing the button.
			   If this exceeds N ticks, then terminate the application. */
			buttonHoldCount++;

			if (buttonHoldCount > kTerminateCount)
			{
				/* Quit, since the user held the button longer than
				   the terminate count. */
				break;
			}
		}
		else if (!currentData.m_buttonState && prevData.m_buttonState)
		{
			/* Reset the button hold count, since the user stopped holding
			   down the stylus button. */
			buttonHoldCount = 0;
		}

		/* Check if an error occurred. */
		if (HD_DEVICE_ERROR(currentData.m_error))
		{
			hduPrintError(stderr, &currentData.m_error, "Device error detected");

			if (hduIsSchedulerError(&currentData.m_error))
			{
				/* Quit, since communication with the device was disrupted. */
				fprintf(stderr, "\nPress any key to quit.\n");
				getch();
				break;
			}
		}

		/* Store off the current data for the next loop. */
		memcpy(&prevData, &currentData, sizeof(DeviceData));
	}
}

/**
 * A callback class for use with the main MQTT client.
 */
class callback : public virtual mqtt::callback
{
public:
	void connection_lost(const string& cause) override {
		cout << "\nConnection lost" << endl;
		if (!cause.empty())
			cout << "\tcause: " << cause << endl;
	}

	void delivery_complete(mqtt::delivery_token_ptr tok) override {
		cout << "\tDelivery complete for token: "
			<< (tok ? tok->get_message_id() : -1) << endl;
	}

	void message_arrived(mqtt::const_message_ptr msg) override {
		string message_payload = msg->to_string();
		std::stringstream ss(message_payload);
		float num;
		std::vector<float> floatArray;

		// 逐个提取数字并存入 vector
		while (ss >> num) {
			floatArray.push_back(num);

			// 如果下一个字符不是空格，跳出循环
			if (ss.peek() != ' ') {
				break;
			}
		}
		//F/T传感器的数据是机器人末端坐标系下的，需要转换到touch基座坐标系下
		Vector3d ee_pose = Vector3d(floatArray[0], floatArray[1], floatArray[2]);
		Vector3d force=Vector3d(floatArray[3], floatArray[4], floatArray[5]);
		Vector3d torque = Vector3d(floatArray[6], floatArray[7], floatArray[8]);
		Matrix3d rotationMatrix_robot_ee;
		rotationMatrix_robot_ee = Eigen::AngleAxisd(ee_pose.norm(), ee_pose.normalized());
		Matrix3d rotationMatrix_touchbase2robotbase;
		rotationMatrix_touchbase2robotbase<< 0.0, 1.0, 0.0,
			0.0,0.0,1.0,
			1.0,0.0,0.0;
		Vector3d touch_force = rotationMatrix_touchbase2robotbase * rotationMatrix_robot_ee * force;
		//扭矩不是这样算的?
		//Vector3d touch_torque = rotationMatrix_touchbase2robotbase * rotationMatrix_robot_ee * torque;
		gForceTorqueData.m_force[0] = (abs(touch_force[0])>3?abs(touch_force[0])-3:0)*(touch_force[0]>0 ? 1 : -1)*0.01;
		gForceTorqueData.m_force[1] = (abs(touch_force[1]) > 3 ? abs(touch_force[1]) - 3 : 0) * (touch_force[1] > 0 ? 1 : -1)*0.05;
		gForceTorqueData.m_force[2] = (abs(touch_force[2]) > 3 ? abs(touch_force[2]) - 3 : 0) * (touch_force[2] > 0 ? 1 : -1)*0.05;
		/*gForceTorqueData.m_force[0] = abs(touch_force[0]) > 3 ? 1*(touch_force[0] > 0 ? 1 : -1):0;
		gForceTorqueData.m_force[1] = abs(touch_force[1]) > 3 ?( 1 * (touch_force[1] > 0 ? 1 : -1)):0;
		gForceTorqueData.m_force[2] = abs(touch_force[2]) > 3 ? 1 * (touch_force[2] > 0 ? 1 : -1):0;*/
		/*gForceTorqueData.m_torque[0] = touch_torque[0];
		gForceTorqueData.m_torque[1] = touch_torque[1];
		gForceTorqueData.m_torque[2] = touch_torque[2];*/
		
	}
};

/*******************************************************************************
 Main function.
 Sets up the device, runs main application loop, cleans up when finished.
*******************************************************************************/
int main(int argc, char* argv[])
{
	HDSchedulerHandle hUpdateHandle = 0;
	HDErrorInfo error;

	/* Initialize the device, must be done before attempting to call any hd
	   functions. */
	HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
	if (HD_DEVICE_ERROR(error = hdGetError()))
	{
		hduPrintError(stderr, &error, "Failed to initialize the device");
		fprintf(stderr, "\nPress any key to quit.\n");
		getch();
		return -1;
	}

	/*Initialize the mqtt*/
	string	address = (argc > 1) ? string(argv[1]) : DFLT_SERVER_ADDRESS,
		clientID = (argc > 2) ? string(argv[2]) : CLIENT_ID;

	cout << "Initializing for server '" << address << "'..." << endl;
	mqtt::async_client client(address, clientID, PERSIST_DIR);

	callback cb;
	client.set_callback(cb);

	auto connOpts = mqtt::connect_options_builder()
		.clean_session()
		.will(mqtt::message(TOPIC, LWT_PAYLOAD, QOS))
		.finalize();

	cout << "  ...OK" << endl;

	cout << "\nConnecting..." << endl;
	mqtt::token_ptr conntok = client.connect(connOpts);
	cout << "Waiting for the connection..." << endl;
	conntok->wait();
	cout << "  ...OK" << endl;
	std::string PAYLOAD;

	client.subscribe("robot_ee_pose&FT", 0)->wait();

	mqtt::message_ptr pubmsg = mqtt::make_message("initial", "initializing");
	pubmsg->set_qos(QOS);
	client.publish(pubmsg)->wait_for(TIMEOUT);

	/* Schedule the main scheduler callback that updates the device state. */
	hUpdateHandle = hdScheduleAsynchronous(
		updateDeviceCallback, 0, HD_MAX_SCHEDULER_PRIORITY);

	hdEnable(HD_FORCE_OUTPUT);

	/* Start the servo loop scheduler. */
	hdStartScheduler();
	if (HD_DEVICE_ERROR(error = hdGetError()))
	{
		hduPrintError(stderr, &error, "Failed to start the scheduler");
		fprintf(stderr, "\nPress any key to quit.\n");
		getch();
		return -1;
	}

	/* Run the application loop. */
	static const int kTerminateCount = 1000;
	int buttonHoldCount = 0;

	/* Instantiate the structure used to capture data from the device. */
	DeviceData currentData;
	DeviceData prevData;

	/* Perform a synchronous call to copy the most current device state. */
	hdScheduleSynchronous(copyDeviceDataCallback,
		&currentData, HD_MIN_SCHEDULER_PRIORITY);

	memcpy(&prevData, &currentData, sizeof(DeviceData));

	cout << "\nSending message" << endl;

	auto startTime = std::chrono::steady_clock::now();
	string button_state = "0";

	//机器人基座到touch基座的旋转矩阵
	Matrix3d rotationMatrix_robot_base2touch_base;
	rotationMatrix_robot_base2touch_base << 0.0, 0.0, 1.0,
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0;
	
	Matrix3d rotationMatrix_touch_ee2robot_ee;
	//touch末端到机器人末端的旋转矩阵(机器人朝向+x轴)
	/*rotationMatrix_touch_ee2robot_ee << -1.0, 0.0, 0.0,
		0.0, 0.0, -1.0,
		0.0, -1.0, 0.0;*/
	//touch末端到机器人末端的旋转矩阵(机器人朝向-y轴)
	rotationMatrix_touch_ee2robot_ee << 0.0, 1.0, 0.0,
		0.0, 0.0, -1.0,
		-1.0, 0.0, 0.0;
	//正确的旋转矩阵(和机器人初始姿态无关，是touch姿态到机器人基座的旋转矩阵)
	/*rotationMatrix_touch_ee2robot_ee << 0.0, 1.0, 0.0,
		0.0, 0.0, 1.0,
		1.0, 0.0, 0.0;*/

	while (1)
	{
		/* Perform a synchronous call to copy the most current device state.
		   This synchronous scheduler call ensures that the device state
		   is obtained in a thread-safe manner. */
		hdScheduleSynchronous(copyDeviceDataCallback,
			&currentData,
			HD_MIN_SCHEDULER_PRIORITY);

		/*fprintf(stdout, "Current position: (%g, %g, %g)\n",
			currentData.m_devicePosition[0],
			currentData.m_devicePosition[1],
			currentData.m_devicePosition[2]
		);*/


		/*std::array<double, 6> current_pose = rawdata2pose(currentData.m_deviceTransform);
		std::array<double,6> prev_pose = rawdata2pose(prevData.m_deviceTransform);*/

		/*Vector3d current_rotationVector = rotationMatrixToVector(rotationMatrix_robot_base2touch_base *rawdataToMatrix(currentData.m_deviceTransform)*rotationMatrix_touch_ee2robot_ee);*/
		Matrix3d previous_rotationMatrix = rawdataToMatrix(prevData.m_deviceTransform) * rotationMatrix_touch_ee2robot_ee;
		Matrix3d current_rotationMatrix = rawdataToMatrix(currentData.m_deviceTransform) * rotationMatrix_touch_ee2robot_ee;
		Matrix3d inverse_rotationMatrix = previous_rotationMatrix.inverse();
		Matrix3d rotationMatrix = inverse_rotationMatrix * current_rotationMatrix;
		Vector3d rotationVector = rotationMatrixToVector(rotationMatrix);

		Vector3d current_position(currentData.m_devicePosition[2], currentData.m_devicePosition[0], currentData.m_devicePosition[1]);
		Vector3d previous_position(prevData.m_devicePosition[2], prevData.m_devicePosition[0], prevData.m_devicePosition[1]);
		Vector3d delta_position = current_position - previous_position;
		/*fprintf(stdout, "Current euler: (%g, %g, %g)\n",
			euler[0],
			euler[1],
			euler[2]
		);*/
		/*std::array<double, 6> pose;
		for (int i = 0; i < 6; ++i) {
			pose[i] = current_pose[i] - prev_pose[i];
		}*/

		if (currentData.m_buttonState && !prevData.m_buttonState)
		{
			button_state = "1";//button down
		}
		else if (!currentData.m_buttonState && prevData.m_buttonState)
		{
			button_state = "0";//button up
		}


		//PAYLOAD = vectorToString(pose)+button_state;//payload is a string of 6 doubles delta pose and a button state
		//delta_position is the delta xyz;delta_position是位移xyz的差值
		//delta pose is(1->2)R=inverse((0->1)R)*(0->2)R,and change the martix(R) to rotation vector;delta pose是姿态1到姿态2的旋转矩阵,然后转换为旋转矢量
		PAYLOAD = std::to_string(delta_position[0]) + " " + std::to_string(delta_position[1]) + " " + std::to_string(delta_position[2]) + " " +
			std::to_string(rotationVector[0]) + " " + std::to_string(rotationVector[1]) + " " + std::to_string(rotationVector[2]) + " " + button_state;
		pubmsg = mqtt::make_message(TOPIC, PAYLOAD);
		client.publish(pubmsg);


		/* Check if an error occurred. */
		if (HD_DEVICE_ERROR(currentData.m_error))
		{
			hduPrintError(stderr, &currentData.m_error, "Device error detected");

			if (hduIsSchedulerError(&currentData.m_error))
			{
				/* Quit, since communication with the device was disrupted. */
				fprintf(stderr, "\nPress any key to quit.\n");
				getch();
				break;
			}
		}

		/* Store off the current data for the next loop. */
		memcpy(&prevData, &currentData, sizeof(DeviceData));


		auto currentTime = std::chrono::high_resolution_clock::now();
		auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(currentTime - startTime);

		// 计算下一个周期应该等待的时间
		auto nextWaitTime = std::chrono::milliseconds(1000) / 80 - elapsedTime;

		// 等待下一个周期
		if (nextWaitTime > std::chrono::milliseconds(0)) {
			std::this_thread::sleep_for(nextWaitTime);
		}

		// 重置开始时间
		startTime = std::chrono::high_resolution_clock::now();
	}

	/* For cleanup, unschedule callbacks and stop the servo loop. */
	hdStopScheduler();
	hdUnschedule(hUpdateHandle);
	hdDisableDevice(hHD);

	return 0;
}

/******************************************************************************/
